#include "Ball.h"
#include <math.h>
#include <stdio.h>
#include <iostream>


//constructor
Ball::Ball(){
	gravity = 3.5;
	initPos.push_back(0);
	initPos.push_back(0);
	initPos.push_back(0);
}
Ball::~Ball(){
}
void Ball::launchBall(vector<double> launchPos, double velocity, double upangle, double bearing, vector<bool> rack){
	uRad = upangle*DEG_TO_RAD;
	bRad = bearing*DEG_TO_RAD;

    vix = velocity*cos(bRad) * cos(uRad);
    viy = velocity*sin(uRad);
	viz = velocity*sin(bRad) * cos(uRad);
	
	//Later, put these in the constructor
	pos.push_back(launchPos[0]);
	pos.push_back(launchPos[1]);
	pos.push_back(launchPos[2]);

	initPos[0] = launchPos[0];
	initPos[1] = launchPos[1];
	initPos[2] = launchPos[2];
	
	
	
}
vector<double> Ball::getPosition(double time){
	double tFactor = time;
	pos[0] = initPos[0] + vix * tFactor;
	pos[1] = initPos[1] + viy *tFactor-.5 * gravity * tFactor*tFactor;  // David -- I messed with gravity
	pos[2] = initPos[2] + viz * tFactor;
	return pos;

}
vector<double> Ball::getAngle(double time){
	vector<double> prevPos = getPosition(time*.8);
	vector<double> curPos = getPosition(time);
	double uRet = (PI/2 - asin((curPos[1]-prevPos[1])/(curPos[2]-prevPos[2])))/DEG_TO_RAD;
	double bRet = (PI/2 - asin((curPos[0]-prevPos[0])/(curPos[2]-prevPos[2])))/DEG_TO_RAD;
	angle.push_back(uRet);
	angle.push_back(bRet);
	//std::cerr << "incang = "<<asin(((double)curPos[2]-(double)prevPos[2])/(double)(curPos[1]-()prevPos[1]))/DEG_TO_RAD<< endl;
	return angle;
}
//const double DEG_TO_RAD = PI / 180.;
//static const double PI = 3.141593;